开发者说 | Apollo 3.5各功能模块启动过程解析
将「技术」真正转化为「生产力」及「产品」,是这一届CES Apollo发布会的新重点。
自动驾驶对计算的高性能和鲁棒性有强烈需求,随着自动驾驶行业逐步从研发转向量产,业内对可用于量产并能兼容自动驾驶系统的强大框架的需求也在增加。
Apollo Cyber RT框架便是为Apollo构建此类框架的第一步,也是第一个专为自动驾驶技术设计的开源框架。
Apollo Cyber RT框架
Apollo Cyber RT 框架核心理念是基于的组件,组件有预先设定的输入输出。
实际上,每个组件就代表一个专用得算法模块。框架可以根据所有预定义的组件生成有向无环图 (DAG)。
在运行时刻,框架把融合好的传感器数据和预定义的组件打包在一起形成用户级轻量任务,之后,框架的调度器可以根据资源可用性和任务优先级来派发这些任务。
Apollo 3.5彻底摒弃ROS,改用自研的Cyber作为底层通讯与调度平台。各功能模块的启动过程与之前版本天壤之别。
感谢社区荣誉布道师—贺博士 对Apollo 3.5 各功能模块的启动过程进行解析(关闭过程可作类似分析,不再赘述),希望给感兴趣的同学带来帮助。
先从启动脚本文件scripts/bootstrap.sh开始剖析。
服务启动命令bash scripts/bootstrap.sh start实际上执行了scripts/bootstrap.sh脚本中的start函数:
1dfunction start() {
2 ./scripts/monitor.sh start
3 ./scripts/dreamview.sh start
4 if [ $? -eq 0 ]; then
5 http_status="$(curl -o -I -L -s -w '%{http_code}' ${DREAMVIEW_URL})"
6 if [ $http_status -eq 200 ]; then
7 echo "Dreamview is running at" $DREAMVIEW_URL
8 else
9 echo "Failed to start Dreamview. Please check /apollo/data/log or /apollo/data/core for more information"
10 fi
11 fi
12}
start函数内部分别调用脚本文件scripts/monitor.sh与scripts/dreamview.sh内部的start函数启动monitor与dreamview模块。
monitor模块的启动过程暂且按下不表,下面专门研究dreamview模块的start函数。scripts/dreamview.sh文件内容如下:
1DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
2
3cd "${DIR}/.."
4
5source "${DIR}/apollo_base.sh"
6
7# run function from apollo_base.sh
8# run command_name module_name
9run dreamview "$@"
里面压根没有start函数,但我们找到一个apollo_base.sh脚本文件,并且有一条调用语句:run dreamview "$@"(展开以后就是run dreamview start)。
我们有理由判断,run函数存在于apollo_base.sh脚本文件,现在到里面一探究竟,不出意外果然有一个run函数:
1function run() {
2 local module=$1
3 shift
4 run_customized_path $module $module "$@"
5}
上述代码中,module的值为dreamview,$@的值为start,因此后面继续调用run_customized_path dreamview dreamview start。继续顺藤摸瓜查看run_customized_path函数:
1function run_customized_path() {
2 local module_path=$1
3 local module=$2
4 local cmd=$3
5 shift 3
6 case $cmd in
7 start)
8 start_customized_path $module_path $module "$@"
9 ;;
10 # ...
11}
实际调用的是start_customized_path dreamview dreamview。
再来查看 start_customized_path函数:
1function start_customized_path() {
2 MODULE_PATH=$1
3 MODULE=$2
4 shift 2
5
6 is_stopped_customized_path "${MODULE_PATH}" "${MODULE}"
7 if [ $? -eq 1 ]; then
8 eval "nohup cyber_launch start /apollo/modules/${MODULE_PATH}/launch/${MODULE}.launch &"
9 sleep 0.5
10 is_stopped_customized_path "${MODULE_PATH}" "${MODULE}"
11 if [ $? -eq 0 ]; then
12 echo "Launched module ${MODULE}."
13 return 0
14 else
15 echo "Could not launch module ${MODULE}. Is it already built?"
16 return 1
17 fi
18 else
19 echo "Module ${MODULE} is already running - skipping."
20 return 2
21 fi
22}
在start_customized_path函数内部,首先调用is_stopped_customized_path函数来判断(在内部实际通过指令$(pgrep -c -f "modules/dreamview/launch/dreamview.launch")来判断)dreamview模块是否已启动。
若该模块未启动,则使用指令
nohup cyber_launch start/apollo/modules/dreamview/launch/dreamview.launch &以非挂断方式启动后台进程模块dreamview。
cyber_launch是Cyber平台提供的一个python工具程序,其完整路径为:
${APOLLO_HOME}/cyber/tools/cyber_launch/cyber_launch。
可通过sudo find / -name cyber_launch查找,${APOLLO_HOME}表示Apollo项目的根目录。
以我的机器为例,Docker外部为/home/davidhopper/code/apollo,Docker内部自不必说,全部为/apollo。为描述简单起见,下文全部以Docker内部的路径/apollo为准。
下面继续研究cyber_launch中的main函数:
1def main():
2 """ main function """
3 cyber_path = os.getenv('CYBER_PATH')
4 if cyber_path == None:
5 logger.error('Error: environment variable CYBER_PATH not found, set environment first.')
6 sys.exit(1)
7 os.chdir(cyber_path)
8 parser = argparse.ArgumentParser(description='cyber launcher')
9 subparsers = parser.add_subparsers(help='sub-command help')
10
11 start_parser = subparsers.add_parser('start', help='launch/benchmark.launch')
12 start_parser.add_argument('file', nargs='?', action='store', help='launch file, default is cyber.launch')
13
14 stop_parser = subparsers.add_parser('stop', help='stop all the module in launch file')
15 stop_parser.add_argument('file', nargs='?', action='store', help='launch file, default stop all the launcher')
16
17 #restart_parser = subparsers.add_parser('restart', help='restart the module')
18 #restart_parser.add_argument('file', nargs='?', action='store', help='launch file, default is cyber.launch')
19
20 params = parser.parse_args(sys.argv[1:])
21
22 command = sys.argv[1]
23 if command == 'start':
24 start(params.file)
25 elif command == 'stop':
26 stop_launch(params.file)
27 #elif command == 'restart':
28 # restart(params.file)
29 else:
30 logger.error('Invalid command %s' % command)
31 sys.exit(1)
32
该函数无非进行一些命令行参数解析,
然后调用start(/apollo/modules/dreamview/launch/dreamview.launch)函数启动dreamview模块。
继续查看start函数,该函数内容很长,不再详细解释,
其主要功能是解析XML文件/apollo/modules/dreamview/launch/dreamview.launch中的各项元素:name、dag_conf、type、process_name、exception_handler,
其值分别为:
dreamview、null、binary、/apollo/bazel-bin/modules/dreamview/dreamview --flagfile
=/apollo/modules/common/data/global_flagfile.txt、respawn,然后调用
ProcessWrapper(process_name.split()[0], 0, [""], process_name, process_type,
exception_handler)创建一个ProcessWrapper对象pw,
然后调用pw.start()函数启动dreamview模块:
1def start(launch_file = ''):
2 # ...
3
4 process_list = []
5 root = tree.getroot()
6 for module in root.findall('module'):
7 module_name = module.find('name').text
8 dag_conf = module.find('dag_conf').text
9 process_name = module.find('process_name').text
10 sched_name = module.find('sched_name')
11 process_type = module.find('type')
12 exception_handler = module.find('exception_handler')
13 # ...
14 if process_name not in process_list:
15 if process_type == 'binary':
16 if len(process_name) == 0:
17 logger.error('Start binary failed. Binary process_name is null')
18 continue
19 pw = ProcessWrapper(process_name.split()[0], 0, [""], process_name, process_type, exception_handler)
20 # default is library
21 else:
22 pw = ProcessWrapper(g_binary_name, 0, dag_dict[str(process_name)], process_name, process_type, sched_name, exception_handler)
23 result = pw.start()
24 if result != 0:
25 logger.error('Start manager [%s] failed. Stop all!' % process_name)
26 stop()
27 pmon.register(pw)
28 process_list.append(process_name)
29
30 # no module in xml
31 if not process_list:
32 logger.error("No module was found in xml config.")
33 return
34 all_died = pmon.run()
35 if not all_died:
36 logger.info("Stop all processes...")
37 stop()
38 logger.info("Cyber exit.")
下面查看ProcessWrapper类里的start函数:
1 def start(self):
2 """
3 start a manager in process name
4 """
5 if self.process_type == 'binary':
6 args_list = self.name.split()
7 else:
8 args_list = [self.binary_path, '-d'] + self.dag_list
9 if len(self.name) != 0:
10 args_list.append('-p')
11 args_list.append(self.name)
12 if len(self.sched_name) != 0:
13 args_list.append('-s')
14 args_list.append(self.sched_name)
15
16 self.args = args_list
17
18 try:
19 self.popen = subprocess.Popen(args_list, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
20 except Exception, e:
21 logger.error('Subprocess Popen exception: ' + str(e))
22 return 2
23 else:
24 if self.popen.pid == 0 or self.popen.returncode is not None:
25 logger.error('Start process [%s] failed.' % self.name)
26 return 2
27
28 th = threading.Thread(target=module_monitor, args=(self, ))
29 th.setDaemon(True)
30 th.start()
31 self.started = True
32 self.pid = self.popen.pid
33 logger.info('Start process [%s] successfully. pid: %d' % (self.name, self.popen.pid))
34 logger.info('-' * 120)
35 return 0
在该函数内部调用/apollo/bazel-bin/modules/dreamview/dreamview --flagfile=/apollo
/modules/common/data/global_flagfile.txt最终启动了dreamview进程。
dreamview进程的main函数位于/apollo/modules/dreamview/backend/main.cc中,内容如下所示:
1 int main(int argc, char *argv[]) {
2 google::ParseCommandLineFlags(&argc, &argv, true);
3 // add by caros for dv performance improve
4 apollo::cyber::GlobalData::Instance()->SetProcessGroup("dreamview_sched");
5 apollo::cyber::Init(argv[0]);
6
7 apollo::dreamview::Dreamview dreamview;
8 const bool init_success = dreamview.Init().ok() && dreamview.Start().ok();
9 if (!init_success) {
10 AERROR << "Failed to initialize dreamview server";
11 return -1;
12 }
13 apollo::cyber::WaitForShutdown();
14 dreamview.Stop();
15 apollo::cyber::Clear();
16 return 0;
17}
该函数初始化Cyber环境,并调用Dreamview::Init()和Dreamview::Start()函数,启动Dreamview后台监护进程。
然后进入消息处理循环,直到等待cyber::WaitForShutdown()返回,清理资源并退出main函数。
Apollo 3.5使用Cyber启动Localization、Perception、Prediction、Planning、Control等功能模块。若只看各模块的BUILD文件,保证你无法找到该模块的启动入口main函数(Apollo 3.5之前的版本均是如此处理)。
下面以Planning模块为例具体阐述。
Planning模块BUILD文件中生成binary文件的配置项如下:
1 cc_binary(
2 name = "libplanning_component.so",
3 linkshared = True,
4 linkstatic = False,
5 deps = [":planning_component_lib"],
6)
该配置项中没有source文件,仅包含一个依赖项:planning_component_lib。又注意到后者的定义如下:
1cc_library(
2 name = "planning_component_lib",
3 srcs = [
4 "planning_component.cc",
5 ],
6 hdrs = [
7 "planning_component.h",
8 ],
9 copts = [
10 "-DMODULE_NAME=\\\"planning\\\"",
11 ],
12 deps = [
13 ":planning_lib",
14 "//cyber",
15 "//modules/common/adapters:adapter_gflags",
16 "//modules/common/util:message_util",
17 "//modules/localization/proto:localization_proto",
18 "//modules/map/relative_map/proto:navigation_proto",
19 "//modules/perception/proto:perception_proto",
20 "//modules/planning/proto:planning_proto",
21 "//modules/prediction/proto:prediction_proto",
22 ],
23)
在srcs文件planning_component.cc以及deps文件中均找不到main函数。
那么main函数被隐藏在哪里?如果没有main函数,binary文件libplanning_component.so又是如何启动的?
答案很简单,planning模块的binary文件libplanning_component.so作为cyber的一个组件启动,不需要main函数。
下面详细阐述在DreamView界面中启动Planning模块的过程。
DreamView前端界面操作此处不表,后端的消息响应函数HMI::RegisterMessageHandlers()位于/apollo/modules/dreamview/backend/hmi/hmi.cc文件中:
1void HMI::RegisterMessageHandlers() {
2
3 // ...
4 websocket_->RegisterMessageHandler(
5 "HMIAction",
6 [this](const Json& json, WebSocketHandler::Connection* conn) {
7 // Run HMIWorker::Trigger(action) if json is {action: ""}
8 // Run HMIWorker::Trigger(action, value) if "value" field is provided.
9 std::string action;
10 if (!JsonUtil::GetStringFromJson(json, "action", &action)) {
11 AERROR << "Truncated HMIAction request.";
12 return;
13 }
14 HMIAction hmi_action;
15 if (!HMIAction_Parse(action, &hmi_action)) {
16 AERROR << "Invalid HMIAction string: " << action;
17 }
18 std::string value;
19 if (JsonUtil::GetStringFromJson(json, "value", &value)) {
20 hmi_worker_->Trigger(hmi_action, value);
21 } else {
22 hmi_worker_->Trigger(hmi_action);
23 }
24
25 // Extra works for current Dreamview.
26 if (hmi_action == HMIAction::CHANGE_MAP) {
27 // Reload simulation map after changing map.
28 CHECK(map_service_->ReloadMap(true))
29 << "Failed to load new simulation map: " << value;
30 } else if (hmi_action == HMIAction::CHANGE_VEHICLE) {
31 // Reload lidar params for point cloud service.
32 PointCloudUpdater::LoadLidarHeight(FLAGS_lidar_height_yaml);
33 SendVehicleParam();
34 }
35 });
36
37 // ...
38}
其中,HMIAction_Parse(action, &hmi_action)用于解析动作参数,
hmi_worker_->Trigger(hmi_action, value)用于执行相关动作。
对于Planning模块的启动而言,
hmi_action的值为HMIAction::START_MODULE,value的值为Planning。
实际上,DreamView将操作模式分为多种hmi mode,
这些模式位于目录/apollo/modules/dreamview/conf/hmi_modes,每一个配置文件均对应一种hmi mode。
但不管处于哪种hmi mode,对于Planning模块的启动而言,
hmi_action的值均为HMIAction::START_MODULE,value的值均为Planning。
当然,Standard Mode与Navigation Mode对应的dag_files不一样,Standard Mode的dag_files为/apollo/modules/planning/dag/planning.dag,Navigation Mode的dag_files为/apollo/modules/planning/dag/planning_navi.dag。
HMIWorker::Trigger(const HMIAction action, const std::string& value)函数位于文件/apollo/modules/dreamview/backend/hmi/hmi_worker.cc中,其内容如下:
1bool HMIWorker::Trigger(const HMIAction action, const std::string& value) {
2 AINFO << "HMIAction " << HMIAction_Name(action) << "(" << value
3 << ") was triggered!";
4 switch (action) {
5 // ...
6 case HMIAction::START_MODULE:
7 StartModule(value);
8 break;
9 // ...
10 }
11 return true;
12}
继续研究HMIWorker::StartModule(const std::string& module)函数:
1void HMIWorker::StartModule(const std::string& module) const {
2 const Module* module_conf = FindOrNull(current_mode_.modules(), module);
3 if (module_conf != nullptr) {
4 System(module_conf->start_command());
5 } else {
6 AERROR << "Cannot find module " << module;
7 }
8}
上述函数中成员变量current_mode_保存着当前hmi mode对应配置文件包含的所有配置项。
例如modules/dreamview/conf/hmi_modes/mkz_standard_debug.pb.txt
里面就包含了MKZ标准调试模式下所有的功能模块,
该配置文件通过HMIWorker::LoadMode(const std::string& mode_config_path)函数读入到成员变量current_mode_中。
如果基于字符串module查找到了对应的模块名以及对应的启动配置文件dag_files,则调用System函数(内部实际调用std::system函数)基于命令module_conf->start_command()启动一个进程。
这个start_command从何而来?
需进一步分析HMIWorker::LoadMode(const std::string& mode_config_path)函数:
1HMIMode HMIWorker::LoadMode(const std::string& mode_config_path) {
2 HMIMode mode;
3 CHECK(common::util::GetProtoFromFile(mode_config_path, &mode))
4 << "Unable to parse HMIMode from file " << mode_config_path;
5 // Translate cyber_modules to regular modules.
6 for (const auto& iter : mode.cyber_modules()) {
7 const std::string& module_name = iter.first;
8 const CyberModule& cyber_module = iter.second;
9 // Each cyber module should have at least one dag file.
10 CHECK(!cyber_module.dag_files().empty()) << "None dag file is provided for "
11 << module_name << " module in "
12 << mode_config_path;
13
14 Module& module = LookupOrInsert(mode.mutable_modules(), module_name, {});
15 module.set_required_for_safety(cyber_module.required_for_safety());
16
17 // Construct start_command:
18 // nohup mainboard -p -d ... &
19 module.set_start_command("nohup mainboard");
20 const auto& process_group = cyber_module.process_group();
21 if (!process_group.empty()) {
22 StrAppend(module.mutable_start_command(), " -p ", process_group);
23 }
24 for (const std::string& dag : cyber_module.dag_files()) {
25 StrAppend(module.mutable_start_command(), " -d ", dag);
26 }
27 StrAppend(module.mutable_start_command(), " &");
28
29 // Construct stop_command: pkill -f '' [0]>
30 const std::string& first_dag = cyber_module.dag_files(0);
31 module.set_stop_command(StrCat("pkill -f \"", first_dag, "\""));
32 // Construct process_monitor_config.
33 module.mutable_process_monitor_config()->add_command_keywords("mainboard");
34 module.mutable_process_monitor_config()->add_command_keywords(first_dag);
35 }
36 mode.clear_cyber_modules();
37 AINFO << "Loaded HMI mode: " << mode.DebugString();
38 return mode;
39}
通过该函数可以看到,构建出的start_command格式为nohup mainboard -p
其中,process_group与dag均来自于当前hmi mode对应的配置文件。
以modules/dreamview/conf/hmi_modes/mkz_close_loop.pb.txt为例,
它包含两个cyber_modules配置项,对于Computer模块而言,它包含了11个dag_files文件(对应11个子功能模块),这些子功能模块全部属于名为compute_sched的process_group。
dag自不必言,每个子功能模块对应一个dag_files,
Planning子功能模块对应的dag_files为/apollo/modules/planning/dag/planning.dag。
1cyber_modules {
2 key: "Computer"
3 value: {
4 dag_files: "/apollo/modules/drivers/camera/dag/camera_no_compress.dag"
5 dag_files: "/apollo/modules/drivers/gnss/dag/gnss.dag"
6 dag_files: "/apollo/modules/drivers/radar/conti_radar/dag/conti_radar.dag"
7 dag_files: "/apollo/modules/drivers/velodyne/dag/velodyne.dag"
8 dag_files: "/apollo/modules/localization/dag/dag_streaming_msf_localization.dag"
9 dag_files: "/apollo/modules/perception/production/dag/dag_streaming_perception.dag"
10 dag_files: "/apollo/modules/perception/production/dag/dag_streaming_perception_trafficlights.dag"
11 dag_files: "/apollo/modules/planning/dag/planning.dag"
12 dag_files: "/apollo/modules/prediction/dag/prediction.dag"
13 dag_files: "/apollo/modules/routing/dag/routing.dag"
14 dag_files: "/apollo/modules/transform/dag/static_transform.dag"
15 process_group: "compute_sched"
16 }
17}
18cyber_modules {
19 key: "Controller"
20 value: {
21 dag_files: "/apollo/modules/canbus/dag/canbus.dag"
22 dag_files: "/apollo/modules/control/dag/control.dag"
23 dag_files: "/apollo/modules/guardian/dag/guardian.dag"
24 process_group: "control_sched"
25 }
26}
27# ...
至此,我们终于找到了Planning功能模块的启动命令为:
1nohup mainboard -p compute_sched -d /apollo/modules/planning/dag/planning.dag &
nohup表示非挂断方式启动,mainboard无疑是启动的主程序,入口main函数必定包含于其中。
process_group的意义不是那么大,无非对功能模块分组而已;dag_files才是我们启动相关功能模块的真正配置文件。
查看cyber模块的构建文件/apollo/cyber/BUILD,可发现如下内容:
1cc_binary(
2 name = "mainboard",
3 srcs = [
4 "mainboard/mainboard.cc",
5 "mainboard/module_argument.cc",
6 "mainboard/module_argument.h",
7 "mainboard/module_controller.cc",
8 "mainboard/module_controller.h",
9 ],
10 copts = [
11 "-pthread",
12 ],
13 linkstatic = False,
14 deps = [
15 ":cyber_core",
16 "//cyber/proto:dag_conf_cc_proto",
17 ],
18)
至此,可执行文件mainboard的踪迹水落石出。
果不其然,入口函数main位于文件cyber/mainboard/mainboard.cc中:
1int main(int argc, char** argv) {
2 google::SetUsageMessage("we use this program to load dag and run user apps.");
3
4 // parse the argument
5 ModuleArgument module_args;
6 module_args.ParseArgument(argc, argv);
7
8 // initialize cyber
9 apollo::cyber::Init(argv[0]);
10
11 // start module
12 ModuleController controller(module_args);
13 if (!controller.Init()) {
14 controller.Clear();
15 AERROR << "module start error.";
16 return -1;
17 }
18
19 apollo::cyber::WaitForShutdown();
20 controller.Clear();
21 AINFO << "exit mainboard.";
22
23 return 0;
24}
main函数十分简单,首先是解析参数,初始化cyber环境,
接下来创建一个ModuleController类对象controller,
之后调用controller.Init()启动相关功能模块。
最后,一直等待cyber::WaitForShutdown()返回,清理资源并退出main函数。
ModuleController::Init()函数十分简单,内部调用了ModuleController::LoadAll()函数:
1bool ModuleController::LoadAll() {
2 const std::string work_root = common::WorkRoot();
3 const std::string current_path = common::GetCurrentPath();
4 const std::string dag_root_path = common::GetAbsolutePath(work_root, "dag");
5
6 for (auto& dag_conf : args_.GetDAGConfList()) {
7 std::string module_path = "";
8 if (dag_conf == common::GetFileName(dag_conf)) {
9 // case dag conf argument var is a filename
10 module_path = common::GetAbsolutePath(dag_root_path, dag_conf);
11 } else if (dag_conf[0] == '/') {
12 // case dag conf argument var is an absolute path
13 module_path = dag_conf;
14 } else {
15 // case dag conf argument var is a relative path
16 module_path = common::GetAbsolutePath(current_path, dag_conf);
17 if (!common::PathExists(module_path)) {
18 module_path = common::GetAbsolutePath(work_root, dag_conf);
19 }
20 }
21 AINFO << "Start initialize dag: " << module_path;
22 if (!LoadModule(module_path)) {
23 AERROR << "Failed to load module: " << module_path;
24 return false;
25 }
26 }
27 return true;
28}
上述函数处理一个dag_conf配置文件循环,
读取配置文件中的所有dag_conf,
并逐一调用
bool ModuleController::LoadModule(const std::string& path)函数加载功能模块:
1bool ModuleController::LoadModule(const std::string& path) {
2 DagConfig dag_config;
3 if (!common::GetProtoFromFile(path, &dag_config)) {
4 AERROR << "Get proto failed, file: " << path;
5 return false;
6 }
7 return LoadModule(dag_config);
8}
上述函数从磁盘配置文件读取配置信息,
并调用bool ModuleController::LoadModule(const DagConfig& dag_config)
函数加载功能模块:
1bool ModuleController::LoadModule(const DagConfig& dag_config) {
2 const std::string work_root = common::WorkRoot();
3
4 for (auto module_config : dag_config.module_config()) {
5 std::string load_path;
6 if (module_config.module_library().front() == '/') {
7 load_path = module_config.module_library();
8 } else {
9 load_path =
10 common::GetAbsolutePath(work_root, module_config.module_library());
11 }
12
13 if (!common::PathExists(load_path)) {
14 AERROR << "Path not exist: " << load_path;
15 return false;
16 }
17
18 class_loader_manager_.LoadLibrary(load_path);
19
20 for (auto& component : module_config.components()) {
21 const std::string& class_name = component.class_name();
22 std::shared_ptr base =
23 class_loader_manager_.CreateClassObj(class_name);
24 if (base == nullptr) {
25 return false;
26 }
27
28 if (!base->Initialize(component.config())) {
29 return false;
30 }
31 component_list_.emplace_back(std::move(base));
32 }
33
34 for (auto& component : module_config.timer_components()) {
35 const std::string& class_name = component.class_name();
36 std::shared_ptr base =
37 class_loader_manager_.CreateClassObj(class_name);
38 if (base == nullptr) {
39 return false;
40 }
41
42 if (!base->Initialize(component.config())) {
43 return false;
44 }
45 component_list_.emplace_back(std::move(base));
46 }
47 }
48 return true;
49}
上述函数看似很长,
核心思想无非是调用class_loader_manager_.LoadLibrary(load_path);
加载功能模块,创建并初始化功能模块类对象,并将该功能模块加入到cyber的组件列表中统一调度管理。
整个Planning模块的启动过程已阐述完毕,但仍有一个问题需要解决:
Planning模块是如何作为Cyber的一个组件注册并动态创建的?
3.1 组件注册过程
首先看组件注册过程。
注意到modules/planning/planning_component.h
的组件类PlanningComponent继承自
cyber::Component<:predictionobstacles>,></prediction::predictionobstacles,>,
里面管理着PlanningBase类对象指针(Apollo 3.5基于场景概念进行规划,目前从PlanningBase类派生出三个规划类:StdPlanning(高精地图模式)、NaviPlanning(实时相对地图模式)、OpenSpacePlanning(自由空间模式),可通过目录modules/planning/dag下的配置文件指定选用何种场景)。
同时,使用宏CYBER_REGISTER_COMPONENT(PlanningComponent)将规划组件PlanningComponent注册到Cyber的组件类管理器。查看源代码可知:
1#define CYBER_REGISTER_COMPONENT(name) \
2 CLASS_LOADER_REGISTER_CLASS(name, apollo::cyber::ComponentBase)
而后者的定义为:
1#define CLASS_LOADER_REGISTER_CLASS(Derived, Base) \
2 CLASS_LOADER_REGISTER_CLASS_INTERNAL_1(Derived, Base, __COUNTER__)
继续展开得到:
1#define CLASS_LOADER_REGISTER_CLASS_INTERNAL_1(Derived, Base, UniqueID) \
2 CLASS_LOADER_REGISTER_CLASS_INTERNAL(Derived, Base, UniqueID)
仍然需要进一步展开:
1#define CLASS_LOADER_REGISTER_CLASS_INTERNAL(Derived, Base, UniqueID) \
2 namespace { \
3 struct ProxyType##UniqueID { \
4 ProxyType##UniqueID() { \
5 apollo::cyber::class_loader::utility::RegisterClass
6 #Derived, #Base); \
7 } \
8 }; \
9 static ProxyType##UniqueID g_register_class_##UniqueID; \
10 }
将PlanningComponent代入上述宏,最终得到:
1 namespace {
2 struct ProxyType__COUNTER__ {
3 ProxyType__COUNTER__() {
4 apollo::cyber::class_loader::utility::RegisterClass
5 "PlanningComponent", "apollo::cyber::ComponentBase");
6 }
7 };
8 static ProxyType__COUNTER__ g_register_class___COUNTER__;
9 }
注意两点:
第一,上述定义位于namespace apollo::planning内;
第二,___COUNTER__是C语言的一个计数器宏,这里仅代表一个占位符,实际展开时可能就是78之类的数字,亦即ProxyType__COUNTER__实际上应为ProxyType78之类的命名。
上述代码简洁明了,首先定义一个结构体ProxyType__COUNTER__,
该结构体仅包含一个构造函数,
在内部调用apollo::cyber::class_loader::utility::RegisterClass
<planningcomponent,< span="">< span="">apollo::cyber::ComponentBase>注册apollo::cyber::
ComponentBase类的派生类PlanningComponent。
并定义一个静态全局结构体
ProxyType__COUNTER__变量:g_register_class___COUNTER__。
继续观察
apollo::cyber::class_loader::utility::RegisterClass函数:
1template <typename Derived, typename Base>
2void RegisterClass(const std::string& class_name,
3 const std::string& base_class_name) {
4 AINFO << "registerclass:" << class_name << "," << base_class_name << ","
5 << GetCurLoadingLibraryName();
6
7 utility::AbstractClassFactory* new_class_factrory_obj =
8 new utility::ClassFactory
9 new_class_factrory_obj->AddOwnedClassLoader(GetCurActiveClassLoader());
10 new_class_factrory_obj->SetRelativeLibraryPath(GetCurLoadingLibraryName());
11
12 GetClassFactoryMapMapMutex().lock();
13 ClassClassFactoryMap& factory_map =
14 GetClassFactoryMapByBaseClass(typeid(Base).name());
15 factory_map[class_name] = new_class_factrory_obj;
16 GetClassFactoryMapMapMutex().unlock();
17}
该函数创建一个模板类
utility::ClassFactory
为其添加类加载器,设置加载库的路径,
最后将工厂类对象加入到ClassClassFactoryMap对象factory_map统一管理。
通过该函数,我们可以清楚地看到,
Cyber使用工厂方法模式完成产品类(例如PlanningComponent)对象的创建:
3.2 动态创建过程
根据第二节内容,
功能模块类PlanningComponent对象在
bool ModuleController::LoadModule(const DagConfig& dag_config)函数内部创建:
1bool ModuleController::LoadModule(const DagConfig& dag_config) {
2 const std::string work_root = common::WorkRoot();
3
4 for (auto module_config : dag_config.module_config()) {
5 std::string load_path;
6 // ...
7 class_loader_manager_.LoadLibrary(load_path);
8 for (auto& component : module_config.components()) {
9 const std::string& class_name = component.class_name();
10 std::shared_ptr
11 class_loader_manager_.CreateClassObj
12 if (base == nullptr) {
13 return false;
14 }
15
16 if (!base->Initialize(component.config())) {
17 return false;
18 }
19 component_list_.emplace_back(std::move(base));
20 }
21
22 // ...
23 }
24 return true;
25}
已经知道,PlanningComponent对象是通过class_loader_manager_.CreateClassObj
现在的问题是:class_loader::ClassLoaderManager与3.1节中的工厂类utility::AbstractClassFactory如何联系起来的?
先看ClassLoaderManager::CreateClassObj函数
(位于文件 cyber/class_loader/class_loader_manager.h 中):
1template <typename Base>
2std::shared_ptr ClassLoaderManager::CreateClassObj(
3 const std::string& class_name) {
4 std::vector
5 for (auto class_loader : class_loaders) {
6 if (class_loader->IsClassValid(class_name)) {
7 return (class_loader->CreateClassObj(class_name));
8 }
9 }
10 AERROR << "Invalid class name: " << class_name;
11 return std::shared_ptr();
12}
上述函数中,从所有class_loaders中找出一个正确的class_loader,
并调用class_loader->CreateClassObj(class_name)
(位于文件cyber/class_loader/class_loader.h中)
创建功能模块组件类对象:
1template <typename Base>
2std::shared_ptr ClassLoader::CreateClassObj(
3 const std::string& class_name) {
4 if (!IsLibraryLoaded()) {
5 LoadLibrary();
6 }
7
8 Base* class_object = utility::CreateClassObj(class_name, this);
9 if (nullptr == class_object) {
10 AWARN << "CreateClassObj failed, ensure class has been registered. "
11 << "classname: " << class_name << ",lib: " << GetLibraryPath();
12 return std::shared_ptr();
13 }
14
15 std::lock_guard<std::mutex> lck(classobj_ref_count_mutex_);
16 classobj_ref_count_ = classobj_ref_count_ + 1;
17 std::shared_ptr classObjSharePtr(
18 class_object, std::bind(&ClassLoader::OnClassObjDeleter, this,
19 std::placeholders::_1));
20 return classObjSharePtr;
21}
上述函数继续调用utility::CreateClassObj(class_name, this)
(位于文件cyber/class_loader/utility/class_loader_utility.h中)
创建功能模块组件类对象:
1template <typename Base>
2Base* CreateClassObj(const std::string& class_name, ClassLoader* loader) {
3 GetClassFactoryMapMapMutex().lock();
4 ClassClassFactoryMap& factoryMap =
5 GetClassFactoryMapByBaseClass(typeid(Base).name());
6 AbstractClassFactory* factory = nullptr;
7 if (factoryMap.find(class_name) != factoryMap.end()) {
8 factory = dynamic_cast<:abstractclassfactory>(
9 factoryMap[class_name]);
10 }
11 GetClassFactoryMapMapMutex().unlock();
12
13 Base* classobj = nullptr;
14 if (factory && factory->IsOwnedBy(loader)) {
15 classobj = factory->CreateObj();
16 }
17
18 return classobj;
19}*>
上述函数使用factory = dynamic_cast<:abstractclassfactory>( factoryMap[class_name])*>;
获取对应的工厂对象指针,
至此终于将class_loader::ClassLoaderManager与3.1节中的工厂类utility::AbstractClassFactory联系起来了。
工厂对象指针找到后,使用classobj = factory->CreateObj();就顺理成章地将功能模块类对象创建出来了。
@平行直线:
也就是说3.5事实上只有两个进程?
@知行合一2018:
Dreamview是一个单独的进程,其他功能模块都是Mainboard加载的进程。
也就是说,每启动一个功能模块,就创建一个mainboard进程,启动10个功能模块就会有10mainboard进程,通过top命令查看便知。
@平行直线:
我还是有点疑问,按照你的意思是说一个.dag对应一个功能模块,即一个进程,这样理解对吗?
但是文章中有一个地方的描述是:
“上述函数处理一个dag_conf配置文件循环,读取配置文件中的所有dag_conf,并逐一调用
bool ModuleController::LoadModule(const std::string& path)函数加载功能模块。”
这句话是指ModuleController::LoadAll()会读dag里面所有componets中的config文件,然后再以组件的方式注册吗?
这里的功能模块是指dag文件中描述的Component还是类似Planning这类?
@知行合一2018:
理解正确。你可以使用top或ps命令查看进程,验证你的猜想。
模块是指dag文件中描述的所有功能模块,而不单指Planning。
自Apollo平台开放已来,我们收到了大量开发者的咨询和反馈,越来越多开发者基于Apollo擦出了更多的火花,并愿意将自己的成果贡献出来,这充分体现了Apollo『贡献越多,获得越多』的开源精神。为此我们开设了『开发者说』板块,希望开发者们能够踊跃投稿,更好地为广大自动驾驶开发者营造一个共享交流的平台!
* 以上内容为开发者原创,不代表百度官方言论。
已获开发者授权,原文地址请戳阅读原文。
﹏﹏﹏﹏﹏﹏﹏﹏ END ﹏﹏﹏﹏﹏﹏﹏﹏